/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "PS2.h"
#include "uart.h"
#include "delay.h"
#include <stdio.h>
#include "oled.h"
#include "pca9685.h"
#include "servo.h"
#include "handle.h"
#include "action.h"
#include "UI.h"
#include "spider_leg_kinematics.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

PS2_TypeDef ps2;

uint8_t receive =0 ;

/* 全局变量定义（《main.pdf》隐含的D1参数：D1(-50,80,0)） */
char command_buffer[20] = {0};
uint8_t command_index = 0;

// 定义命令队列，用于处理并发命令
typedef enum {
  CMD_NONE,
  CMD_MOVE_FORWARD,
  CMD_MOVE_BACKWARD,
  CMD_MOVE_LEFT,
  CMD_MOVE_RIGHT,
  CMD_TURN_LEFT,
  CMD_TURN_RIGHT,
  CMD_SQUAT_STAND,
  CMD_JUMP,
  CMD_BODY_SWAY1,
  CMD_BODY_SWAY0,
  CMD_BODY_TWIST_NEG,
  CMD_BODY_TWIST_POS,
  CMD_BODY_WAVE,
  CMD_NOD_HEAD,
  CMD_DANCE,              // 新增的舞蹈命令
  CMD_HANDSHAKE,          // 新增的握手命令
} command_type_t;

#define COMMAND_QUEUE_SIZE 10
command_type_t command_queue[COMMAND_QUEUE_SIZE] = {CMD_NONE};
uint8_t command_queue_head = 0;
uint8_t command_queue_tail = 0;
uint8_t command_queue_count = 0;

// 命令队列操作函数
uint8_t command_queue_push(command_type_t cmd) {
  if (command_queue_count >= COMMAND_QUEUE_SIZE) {
    return 0; // 队列满
  }

  command_queue[command_queue_head] = cmd;
  command_queue_head = (command_queue_head + 1) % COMMAND_QUEUE_SIZE;
  command_queue_count++;
  return 1;
}

command_type_t command_queue_pop(void) {
  if (command_queue_count == 0) {
    return CMD_NONE; // 队列空
  }

  command_type_t cmd = command_queue[command_queue_tail];
  command_queue_tail = (command_queue_tail + 1) % COMMAND_QUEUE_SIZE;
  command_queue_count--;
  return cmd;
}
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
  if (huart->Instance == USART2) {
    // 发送回显
    // 直接处理接收到的十六进制命令字节
    uint8_t cmd_byte = receive; // 接收的十六进制命令字节
    command_type_t cmd = CMD_NONE;

    // 根据十六进制值映射到对应命令
    switch(cmd_byte) {
      case 0x01: cmd = CMD_MOVE_FORWARD; break;       // 前进
      case 0x02: cmd = CMD_MOVE_BACKWARD; break;      // 后退
      case 0x03: cmd = CMD_MOVE_LEFT; break;          // 左移
      case 0x04: cmd = CMD_MOVE_RIGHT; break;         // 右移
      case 0x05: cmd = CMD_TURN_LEFT; break;          // 左转
      case 0x06: cmd = CMD_TURN_RIGHT; break;         // 右转
      case 0x07: cmd = CMD_SQUAT_STAND; break;        // 蹲下站立
      case 0x08: cmd = CMD_JUMP; break;               // 跳跃
      case 0x09: cmd = CMD_BODY_SWAY1; break;         // 身体摇摆1
      case 0x0A: cmd = CMD_BODY_SWAY0; break;         // 身体摇摆0
      case 0x0B: cmd = CMD_BODY_TWIST_NEG; break;     // 身体旋转负
      case 0x0C: cmd = CMD_BODY_TWIST_POS; break;     // 身体旋转正
      case 0x0D: cmd = CMD_BODY_WAVE; break;          // 身体波动
      case 0x0E: cmd = CMD_NOD_HEAD; break;           // 点头
      case 0x0F: cmd = CMD_DANCE; break;              // 舞蹈
      case 0x10: cmd = CMD_HANDSHAKE; break;          // 握手
      default: cmd = CMD_NONE; // 未知命令
    }

    // 将命令添加到队列
    if (cmd != CMD_NONE) {
      if (!command_queue_push(cmd)) {
        printf("命令队列已满，丢弃命令: 0x%02X\r\n", cmd_byte);
      } else {
        printf("已添加命令到队列: 0x%02X\r\n", cmd_byte);
      }
    } else {
      printf("未知命令: 0x%02X\r\n", cmd_byte);
    }

    // 重新启动接收中断
    HAL_UART_Receive_IT(&huart2, &receive, 1);
  }
}



/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_I2C1_Init();
  MX_I2C2_Init();
  MX_TIM3_Init();
  MX_USART2_UART_Init();
  MX_TIM2_Init();
  MX_TIM1_Init();
  /* USER CODE BEGIN 2 */


  printf("hello world!\r\n");
  delay_init();
  // PS2_SetInit();		 							//=====ps2配置初始化,配置“红绿灯模式”，并选择是否可以修改
  AX_PS2_Init();
   OLED_Init();
   PCA9685_Init(50.0, 90); // 50Hz频率，初始角度90度
   Servo_Init();
  action_stand();
  delay_ms(1000);
  OLED_NewFrame();
   OLED_DrawImage(30, -20, &终极蜘蛛侠Img, OLED_COLOR_REVERSED);
  OLED_PrintString(15, 32, "按下SELECT", &font15x15, OLED_COLOR_NORMAL);
  OLED_PrintString(15, 50, "进入控制模式", &font15x15, OLED_COLOR_NORMAL);
  OLED_ShowFrame();

  HAL_UART_Receive_IT(&huart2, &receive, 1);
  HAL_TIM_Base_Start_IT(&htim2);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {




    //
     UI_Process();
    // 从命令队列中取出命令并执行
    command_type_t current_cmd = command_queue_pop();
    switch (current_cmd) {
      case CMD_MOVE_FORWARD:
       // printf("执行前进动作\r\n");
        action_move_forward(50, 60);
        break;
      case CMD_MOVE_BACKWARD:
       // printf("执行后退动作\r\n");
        action_move_backward(50, 60);
        break;
      case CMD_MOVE_LEFT:
        //printf("执行左移动作\r\n");
        action_move_left(50, 60);
        break;
      case CMD_MOVE_RIGHT:
       // printf("执行右移动作\r\n");
        action_move_right(50, 60);
        break;
      case CMD_TURN_LEFT:
        //printf("执行左转动作\r\n");
        action_turn_left(90, 72);
        break;
      case CMD_TURN_RIGHT:
        //printf("执行右转动作\r\n");
        action_turn_right(90, 72);
        break;
      case CMD_SQUAT_STAND:
       // printf("执行蹲下站立动作\r\n");
        action_squat_stand(4, 30);
        break;
      case CMD_JUMP:
       // printf("执行跳跃动作\r\n");
        action_jump(4, 50);
        break;
      case CMD_BODY_SWAY1:
       // printf("执行身体摇摆1动作\r\n");
        for (int i = 0; i < 5; i++) {  // 执行5次循环，你可以根据需要调整次数
          action_body_sway(1, 40, 30);
        }
        break;
      case CMD_BODY_SWAY0:
        //printf("执行身体摇摆0动作\r\n");
        for (int i = 0; i < 5; i++) {  // 执行5次循环，你可以根据需要调整次数
          action_body_sway(0, 40, 30);
        }
        break;
      case CMD_BODY_TWIST_NEG:
        //printf("执行身体旋转负动作\r\n");
        for (int i = 0; i < 5; i++) {  // 执行5次循环，你可以根据需要调整次数
          action_body_twist(-15.0f, 30);
        }
        break;
      case CMD_BODY_TWIST_POS:
        //printf("执行身体旋转正动作\r\n");
        for (int i = 0; i < 5; i++) {  // 执行5次循环，你可以根据需要调整次数
          action_body_twist(15.0f, 30);
        }

        break;
      case CMD_BODY_WAVE:
       // printf("执行身体波动动作\r\n");
        for (int i = 0; i < 5; i++) {  // 执行5次循环，你可以根据需要调整次数
          action_body_wave(18, 30);
        }
        break;
      case CMD_NOD_HEAD:
        //printf("执行点头动作\r\n");
        action_nod_head(5, 30);
        break;
      case CMD_DANCE:
        //printf("执行舞蹈动作\r\n");
        // 综合舞蹈表演，结合多种动作
        // 1. 先点头问好
        action_nod_head(4, 30);
        delay_ms(500);

        // 2. 身体摇摆
        for(int i = 0; i < 3; i++) {
          action_body_sway(1, 40, 30);  // 左右摇摆
          action_body_sway(0, 40, 30);  // 前后摇摆
        }

        delay_ms(500);

        // 3. 身体旋转
        action_body_twist(-15.0f, 30);  // 向负方向旋转
        action_body_twist(15.0f, 30);   // 向正方向旋转

        delay_ms(500);

        // 4. 身体波动
        for (int i = 0; i < 6; i++) {
          action_body_wave(18, 30);
        }

        delay_ms(500);

        // 5. 蹲下站立
        action_squat_stand(2, 30);

        delay_ms(500);

        // 6. 跳跃
        action_jump(2, 50);

        delay_ms(500);
        action_robot_dance(3, 30);
        delay_ms(500);
        action_disco_dance(3, 30);

        // 7. 再次点头致谢
        action_nod_head(4, 30);
        //printf("舞蹈表演结束\r\n");
        break;
        case CMD_HANDSHAKE:
         // printf("执行握手动作\r\n");
          action_wave_hand(10, 30);
          break;

      default:
        break;
    }



    // 确保动作执行函数被调用
    action_move_execute();







   //  action_move_forward(50, 60);
   //  delay_ms(1000);
   //  action_move_backward(50, 60);
   //  delay_ms(1000);
   //  action_move_left(50, 60);
   // delay_ms(1000);
   //  action_move_right(50, 72);
   //   delay_ms(1000);
   //  action_turn_left(90, 72);
   //  delay_ms(1000);
   //  action_turn_right(90, 72);
   //  delay_ms(1000);




    // action_nod_head(5, 5);


    //action_wave_dance(1, 5);

     // action_handshake(50);

    // action_stretch(5 );
     //action_cross_step(5, 10);
     //action_spiral_twist(4, 10);
     //action_wave_hand(1, 50);
     //action_robot_dance(1, 10);
     //action_wave_dance(1, 5);
    // action_disco_dance(1, 10);


    // action_body_wave(15, 5);
    // action_body_twist(-15.0f, 5);
    // action_body_twist(15.0f, 5);
    // action_body_sway(0, 40, 5);
    // action_body_sway(1, 40, 5);
    // action_jump(1, 10);
    // action_squat_stand(5, 10);
    // action_tip_toe(5, 10);












    // action_twist_waist_continuous(-1, 2000);
    // delay_ms(1000);
    // action_twist_waist_stop();

    //逆解算
    //根据全局坐标计算关节角度
    //leg_move_to_global_position(1, 111.0f, 87.0f, -66.0f);  // 全局坐标










//     // // delay_ms(1000);
//     // 1. 正解验证：输入{0,0,0}→输出{70.6,0,-89.07}
//    ServoAngles init_ang = {0.0f, 30.0f, 40.0f};
//     leg_handle(1,0,30,40);
//     leg_handle(2,0,30,40);
//     leg_handle(3,0,30,40);
//     leg_handle(4,0,30,40);
//     leg_handle(5,0,30,40);
//     leg_handle(6,0,30,40);

//     D4Coord init_coord;
//     Leg_ForwardKinematics(init_ang, &init_coord);
//     // 4. 可以测试多条腿的全局坐标
//     for (int leg_idx = 0; leg_idx < 6; leg_idx++) {
//       D4Coord leg_global_coord;
//       Leg_ForwardKinematicsGlobal(leg_idx, init_ang, &leg_global_coord);
//       printf("腿%d全局坐标: x=%.2f, y=%.2f, z=%.2f mm\n",
//              leg_idx+1, leg_global_coord.x, leg_global_coord.y, leg_global_coord.z);
//     }

//     delay_ms(1000);
    // // 3. 打印结果进行对比
    // printf("局部坐标: x=%.2f, y=%.2f, z=%.2f mm\n",
    //        init_coord.x, init_coord.y, init_coord.z);
    // printf("全局坐标(腿1): x=%.2f, y=%.2f, z=%.2f mm\n",
    //        global_coord.x, global_coord.y, global_coord.z);


//
// for (float i=60;i<140;i++){
//        // 2. 逆解验证：输入上面计算出的坐标→输出角度
//         D4Coord target_coord = {i, 0.00f, -60.23f };
//         ServoAngles calc_ang;
//         if (Leg_InverseKinematics(target_coord, &calc_ang) == LEG_OK) {
//           // 3. 控制舵机：将计算出的角度传递给leg_handle函数
//           // 控制第一条腿（legId=1）
//           leg_handle(1,
//                     (int16_t)calc_ang.body,   // 髋关节角度
//                     (int16_t)calc_ang.limb,   // 膝关节角度
//                     (int16_t)calc_ang.foot);  // 踝关节角度
//         }
//        delay_ms(100);
//      }











  //   // 1. 定义关节角度（示例：初始态，body=0°、limb=0°、foot=0°，与 main.pdf 初始态一致）
  //   Leg_JointsTypeDef test_joints = {
  //     .body = -40.0f,  // 髋关节角度（对应 main.pdf 中 θ₁=0）
  //     .limb = -30.0f,  // 膝关节角度（对应 main.pdf 中 θ₂=0）
  //     .foot = 50.0f   // 踝关节角度（对应 main.pdf 中 θ₃=0）
  // };
  //
  //   // 2. 定义 D4 坐标结构体（存储正解结果）
  //   D4_CoordTypeDef test_coord;
  //
  //   // 3. 调用正解函数（输入关节角，输出 D4 坐标）
  //   Leg_ForwardKinematics(&test_joints, &test_coord);
  //
  //   // 4. 结果输出（通过串口打印，验证与 main.pdf 初始态 D4 坐标一致）
  //   printf("正解结果：D4(x=%.2f mm, y=%.2f mm, z=%.2f mm)\r\n",
  //          test_coord.x, test_coord.y, test_coord.z);

  //
  //   // 1. 定义目标 D4 坐标（示例：让 D4 向前移动 10 mm，y=10 mm，其他与初始态一致）
  //   D4_CoordTypeDef target_coord = {
  //     .x = 30.00f,  // x 轴保持初始值（与 main.pdf 初始态 x 一致）
  //     .y = 10.00f,  // y 轴向前移动 10 mm（xoy 平面正向）
  //     .z = -60.00f  // z 轴保持初始高度（与 main.pdf 初始态 z 一致）
  // };
  //
  //   // 2. 定义关节角度结构体（存储逆解结果）
  //   Leg_JointsTypeDef target_joints;
  //
  //   // 3. 调用逆解函数（输入目标坐标，输出关节角）
  //   if (Leg_InverseKinematics(&target_coord, &target_joints) == 1) {
  //     // 3.1 逆解成功：打印关节角度（参考 main.pdf 逆解验证逻辑）
  //     printf("逆解成功：body=%.1f°, limb=%.1f°, foot=%.1f°\r\n",
  //            target_joints.body, target_joints.limb, target_joints.foot);
  //
  //     // 3.2 调用 leg_handle 控制舵机（将逆解结果传入，驱动单腿运动）
  //     leg_handle(1,                // 第一条腿
  //                (int16_t)target_joints.body,  // 髋关节角度（转为 int16_t 适配函数参数）
  //                (int16_t)target_joints.limb,  // 膝关节角度
  //                (int16_t)target_joints.foot); // 踝关节角度
  //   } else {
  //     // 3.3 逆解失败：目标坐标超出单腿运动范围（如 main.pdf 中“逆解可行性判断”）
  //     printf("逆解失败：目标 D4 坐标超出单腿运动范围\r\n");
  //   }
    // 预期结果：x=70.60 mm（28.0+42.6）、y=0.00 mm、z=-89.07 mm（-L3），与 main.pdf 初始态完全匹配
   // simple_forward();
    //stand_mark();
  //  stand_action();





   // delay_ms(1000);
     // Servo(1, 90);
     // Servo(2, 90);
     // Servo(3, 90);
     // Servo(4, 90);
     // Servo(5, 90);
     // Servo(6, 90);
     //
     // Servo(7, 90);
     // Servo(8, 90);
     // Servo(9, 90);
     // Servo(10, 90);
     // Servo(11, 90);
     // Servo(12, 90);
     //
     // Servo(13, 90);
     // Servo(14, 90);
     // Servo(15, 90);
     // Servo(16, 90);
     // Servo(17, 90);
     // Servo(18, 90);



    // delay_ms(1000);

    // //控制单个舵机
    // Servo_SetAngle(TIM_CHANNEL_3, 90);   // 通道3舵机转到45度
    // Servo_SetAngle(TIM_CHANNEL_4, 90);  // 通道4舵机转到135度
    // delay_ms(1000);
    // Servo_SetAngle(TIM_CHANNEL_3, 180);   // 通道3舵机转到45度
    // Servo_SetAngle(TIM_CHANNEL_4, 180);  // 通道4舵机转到135度


    // //控制舵机
    // setAngle(6, 0);  // 设置通道0的舵机角度为45度
    // Servo_SetAngle(TIM_CHANNEL_3, 90);   // 通道3舵机转到45度
    // Servo_SetAngle(TIM_CHANNEL_4, 90);  // 通道4舵机转到135度
    // delay_ms(1000);  // 延时1秒
    // setAngle(6, 180); // 设置通道0的舵机角度为135度
    // Servo_SetAngle(TIM_CHANNEL_3, 180);   // 通道3舵机转到45度
    // Servo_SetAngle(TIM_CHANNEL_4, 180);  // 通道4舵机转到135度
    // delay_ms(1000);  // 延时1秒




    // for (uint8_t i = 0; i < 256; i++) {
    //   OLED_NewFrame();
    //   OLED_DrawImage((128 - (bilibiliImg.w)) / 2, 0, &bilibiliImg, OLED_COLOR_NORMAL);
    //   OLED_PrintString(128 - i, 64 - 16, "波特律动Hello", &font16x16, OLED_COLOR_NORMAL);
    //   OLED_ShowFrame();
    // }
    // PS2_LX=PS2_AnologData(PSS_LX);
    // PS2_LY=PS2_AnologData(PSS_LY);
    // PS2_RX=PS2_AnologData(PSS_RX);
    // PS2_RY=PS2_AnologData(PSS_RY);
    // PS2_KEY=PS2_DataKey();


    // switch(PS2_KEY)
    // {
    //   case 1:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 2:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 3:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 4:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 5:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 6:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 7:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 8:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 9:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 10:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 11:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 12:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 13:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 14:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 15:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   case 16:  PS2_Vibration(0x01, 0xFF);  // 最大强度震动
    //           HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin,
    //           HAL_GPIO_ReadPin(LED_GPIO_Port, LED_Pin) ? GPIO_PIN_RESET : GPIO_PIN_SET);
    //     break;
    //   default:
    //     break;
    // }

    // printf("PS2左X轴:%d,PS2左Y轴:%d,PS2右X轴:%d,PS2右Y轴:%d,PS2按键:%d,MOD:%d \r\n",PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY,PS2_RedLight());
    // delay_ms(10);



    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if (htim == &htim2) {
    AX_PS2_ScanKey(&ps2);
    PS2_LX = -ps2.L_X + 128;
    PS2_LY = -ps2.L_Y + 127;
    PS2_RX = -ps2.R_X + 128;
    PS2_RY = -ps2.R_Y + 127;
    // //printf("MODE:%d BTN1:%d BTN2:%d R_LR:%d R_UD:%d L_LR:%d L_UD:%d\r\n",
    // ps2.mode, ps2.btn1, ps2.btn2,
    // PS2_RX, PS2_RY, PS2_LX, PS2_LY);
  }
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
